#include "nodePool3D.h"
#include "mathFunc.h"
#include <stdlib.h>
#include <stdio.h>
#include <assert.h>
#include <iostream>
#include <math.h>

nodePool3D::nodePool3D(octNode *_root)
{
	_dim = int(ceil(powf(float(_root->getNonLeafNodeNumInSubTree()),1.0f/3.0f )));
	static int count=0; 

	_metaData = (nodeMetaData*)malloc(_dim*2*_dim*2*_dim*2*sizeof(nodeMetaData));
	memset(_metaData,0,_dim*2*_dim*2*_dim*2*sizeof(nodeMetaData));
	_data = (nodeTexel*)malloc(_dim*2*_dim*2*_dim*2*sizeof(nodeTexel));
	memset(_data, 0, _dim*2*_dim*2*_dim*2*sizeof(nodeTexel));

	//gather octree data into metaPool 
	setSelfNodeClusterPointer(_root, count);
	_count = count;

	createMetaNodePoolFromTree(_root);

	generateCompressedNodePool();


}
void nodePool3D::setSelfNodeClusterPointer(octNode* node, int &count)
{
	if(!node->isLeaf())
	{
		int indexZ = count/(_dim*_dim);
		int indexY = (count-indexZ*(_dim*_dim))/_dim;
		int indexX = count - indexZ*(_dim*_dim) - indexY*_dim;
				
		//std::cout<<" Set Self Node Cluster Pointer"<<std::endl;
		std::cout<<"count:"<<count<<"  index: "<<indexZ<<" "<<indexY<<" "<<indexX<<std::endl;
		count++;
		//setup self clusterPointer
		node->nodeInfo.selfNodeClusterPointer[2] = indexZ;
		node->nodeInfo.selfNodeClusterPointer[1] = indexY;
		node->nodeInfo.selfNodeClusterPointer[0] = indexX;//!!!!

		for(int i=0; i<2; i++)
			for (int j=0; j<2; j++)
				for(int k=0; k<2; k++)
						setSelfNodeClusterPointer(node->getChild(k,j,i), count);
	}
}

void nodePool3D::createMetaNodePoolFromTree(octNode *node)
{

	if(!node->isLeaf() ) //make sure is not leaf
	{
		//must first setup the selfNodeClusterPointer
		int z = node->nodeInfo.selfNodeClusterPointer[2];
		int y = node->nodeInfo.selfNodeClusterPointer[1];
		int x = node->nodeInfo.selfNodeClusterPointer[0];

		setMetaNodeClusterFromOctNode(z, y, x, node);

		for(int i=0; i<2; i++)
			for (int j=0; j<2; j++)
				for(int k=0; k<2; k++)
					createMetaNodePoolFromTree(node->getChild(k,j,i));


	}

}

void nodePool3D::setMetaNodeClusterFromOctNode(int z, int y, int x, octNode* inode)
{
	for(int k=0; k<2; k++)
		for (int j=0; j<2; j++)
			for(int i=0; i<2; i++)
			{
				nodeMetaData metaData;
				octNode* node = inode->getChild(k,j,i);
				assert(node);
				metaData.ID = node->nodeInfo.ID;
				//block pointer
				metaData.blockPointer[0] = node->nodeInfo.blockPointer[0];
				metaData.blockPointer[1] = node->nodeInfo.blockPointer[1];
				metaData.blockPointer[2] = node->nodeInfo.blockPointer[2];

				//children node cluster pointer
				metaData.childrenNodeClusterPointer[0] = node->nodeInfo.selfNodeClusterPointer[0];
				metaData.childrenNodeClusterPointer[1] = node->nodeInfo.selfNodeClusterPointer[1];
				metaData.childrenNodeClusterPointer[2] = node->nodeInfo.selfNodeClusterPointer[2];

				//subDiviable
				if(node->isLeaf())
					metaData.subD = 0;
				else
					metaData.subD = 1;

				//type of the node
				if(!node->isSingleColor())
					metaData.type = 1;
				else
					metaData.type = 0;

				//pointer to the tree
				assert(node);
				metaData.octNodePointer = node;

				//timeStamp
				metaData.timeStamp = 0;

				setSingleMetaNode(z*2+k, y*2+j, x*2+i, metaData);
			}
}


void nodePool3D::setSingleMetaNode(int z, int y, int x, nodeMetaData metaData)
{
	assert(z<_dim*2 && y<_dim*2 && x<_dim*2);
	int ddim = _dim*2;
	_metaData[z*ddim*ddim+y*ddim+x] = metaData;
}

nodeMetaData nodePool3D::getSingleMetaNode(int z, int y, int x)
{
	assert(z<_dim*2 && y<_dim*2 && x<_dim*2);
	int ddim = _dim*2;
	nodeMetaData metaData = _metaData[z*ddim*ddim+y*ddim+x];
	return metaData;
}

void nodePool3D::generateCompressedNodePool()
{
	for(int k=0; k<_dim*2; k++)
		for (int j=0; j<_dim*2; j++)
			for(int i=0; i<_dim*2; i++)
			{
				setNodeTexel(k,j,i, convertToCompressedNode(getSingleMetaNode(k,j,i)));
	//			nodeMetaData metaNode = convertToMetaDataNode(getNodeTexel(k,j,i));
	//printf("childCluster: %d %d %d - SubD %d -type %d - blockPointer: %d %d %d \n",metaNode.childrenNodeClusterPointer[2],
	//	metaNode.childrenNodeClusterPointer[1],metaNode.childrenNodeClusterPointer[0], metaNode.subD, metaNode.type, metaNode.blockPointer[2],
	//	metaNode.blockPointer[1], metaNode.blockPointer[0]);
			}
}

void nodePool3D::generateMetaNodePool()
{
	for(int k=0; k<_dim*2; k++)
		for (int j=0; j<_dim*2; j++)
			for(int i=0; i<_dim*2; i++)
			{
				setSingleMetaNode(k,j,i, convertToMetaDataNode(getNodeTexel(k,j,i)));
	//			nodeMetaData metaNode = getSingleMetaNode(k,j,i);
	//printf("childCluster: %d %d %d - SubD %d - blockPointer: %d %d %d \n",metaNode.childrenNodeClusterPointer[2],
	//	metaNode.childrenNodeClusterPointer[1],metaNode.childrenNodeClusterPointer[0], metaNode.subD, metaNode.blockPointer[2],
	//	metaNode.blockPointer[1], metaNode.blockPointer[0]);

			}
}

nodeTexel nodePool3D::convertToCompressedNode(nodeMetaData metaNode) //#checked
{
	nodeTexel node;
	node.first = 0; node.second = 0;
	if(metaNode.octNodePointer!=0)
	{
	//printf("childCluster: %d %d %d - SubD %d - blockPointer: %d %d %d \n",metaNode.childrenNodeClusterPointer[2],
	//	metaNode.childrenNodeClusterPointer[1],metaNode.childrenNodeClusterPointer[0], metaNode.subD, metaNode.blockPointer[2],
	//	metaNode.blockPointer[1], metaNode.blockPointer[0]);
		
	if(metaNode.type)
		setBlockNodeTexel(node, int(metaNode.subD),metaNode.childrenNodeClusterPointer[2],
		metaNode.childrenNodeClusterPointer[1], metaNode.childrenNodeClusterPointer[0],
		metaNode.blockPointer[2],metaNode.blockPointer[1],metaNode.blockPointer[0]);
	else
		//TODO: only works for char
		setSingleColorNodeTexel(node, int(metaNode.subD), metaNode.childrenNodeClusterPointer[2],
		metaNode.childrenNodeClusterPointer[1], metaNode.childrenNodeClusterPointer[0],unsigned int(metaNode.octNodePointer->getVolumeSingleValue()*255));
	}
	return node;
}

nodeMetaData nodePool3D::convertToMetaDataNode(nodeTexel node)
{
	nodeMetaData metaData;
	memset(&metaData,0,sizeof(metaData));
		
	getNodePos(node, metaData.childrenNodeClusterPointer[2],metaData.childrenNodeClusterPointer[1],metaData.childrenNodeClusterPointer[0]);	
	getNodeTypeFlag(node, metaData.type);

	getMaxSubDivision(node, metaData.subD);

	if(metaData.type)
		getBlockPos(node,metaData.blockPointer[2],metaData.blockPointer[1],metaData.blockPointer[0]);
	else
		;
	//printf("childCluster: %d %d %d - SubD %d -type %d - blockPointer: %d %d %d \n",metaData.childrenNodeClusterPointer[2],
	//	metaData.childrenNodeClusterPointer[1],metaData.childrenNodeClusterPointer[0], metaData.subD, metaData.type,
	//	metaData.blockPointer[2],metaData.blockPointer[1], metaData.blockPointer[0]);



	return metaData;

}

nodePool3D::nodePool3D(int clusterDim)
{
	_dim = clusterDim;
	_data = (nodeTexel*)malloc(clusterDim*2*clusterDim*2*clusterDim*2*sizeof(nodeTexel));
	_metaData = 0;
}

nodePool3D::nodePool3D()
{
	_data = 0;
	_metaData = 0;
}

nodePool3D::~nodePool3D()
{
	if(_data)
		free(_data);
	if(_metaData)
		free(_metaData);
}

void nodePool3D::setNodeTexel(int z, int y, int x, nodeTexel node) //#checked
{
	int ddim = _dim*2;
	assert(z<ddim); 	assert(y<ddim); 	assert(x<ddim);
	_data[z*ddim*ddim + y*ddim + x] = node;
}

nodeTexel nodePool3D::getNodeTexel(int z, int y, int x) //#checked
{
	int ddim = _dim*2;
	assert(z<ddim); 	assert(y<ddim); 	assert(x<ddim);
	return _data[z*ddim*ddim + y*ddim + x];
}

//void nodePool3D::setNodeCluster(int z, int y, int x, nodeCluster nc)
//{
//	z = z*2; 	y = y*2; 	x = x*2;
//
//	setSingleNode(z, y, x, nc.node[0][0][0]); setSingleNode(z,y,x+1, nc.node[0][0][1]);
//	setSingleNode(z,y+1,x, nc.node[0][1][0]); setSingleNode(z,y+1,x+1, nc.node[0][1][1]);
//	
//	setSingleNode(z+1,y,x,nc.node[1][0][0]); setSingleNode(z+1,y,x+1,nc.node[1][0][1]);
//	setSingleNode(z+1,y+1,x,nc.node[1][1][0]);setSingleNode(z+1,y+1,x+1,nc.node[1][1][1]);
//
//}

//nodeCluster nodePool3D::getNodeCluster(int z, int y, int x)
//{
//	nodeCluster cluster;
//	z = z*2; 	y = y*2; 	x = x*2;
//	cluster.node[0][0][0] = getNodeTexel(z,y,x); cluster.node[0][0][1] = getNodeTexel(z,y,x+1);
//	cluster.node[0][1][0] = getNodeTexel(z,y+1,x); cluster.node[0][1][1] = getNodeTexel(z,y+1,x+1);
//	cluster.node[1][0][0] = getNodeTexel(z+1,y,x); cluster.node[1][0][1] = getNodeTexel(z+1,y,x+1);
//	cluster.node[1][1][0] = getNodeTexel(z+1,y+1,x); cluster.node[1][1][1] = getNodeTexel(z+1,y+1,x+1);
//	return cluster;
//}
//

//assign and retrive data
//get
void nodePool3D::getNodePos(nodeTexel node, unsigned int &z, unsigned int &y, unsigned int &x )
{
	x = node.first & 0x3FF ;
	y = (node.first>>10) & 0x3FF;
	z = (node.first>>20) & 0x3FF;
	
}

void nodePool3D::getBlockPos(nodeTexel node, unsigned int &z, unsigned int &y, unsigned int &x)
{
	x = node.second & 0x3FF ;
	y = (node.second>>10) & 0x3FF;
	z = (node.second>>20) & 0x3FF;
}

void nodePool3D::getRGBA(nodeTexel node, int &R, int &G, int &B, int &A )
{
	A = node.second & 0xFF;
	B = (node.second>>8) & 0xFF;
	G = (node.second>>16) & 0xFF;
	R = (node.second>>24) & 0xFF;
		
}

void nodePool3D::getSingleValue(nodeTexel node, unsigned int &value)
{
	value = node.second;
}

void nodePool3D::getMaxSubDivision(nodeTexel node, bool &subD)
{
	subD = ((node.first>>31) & 0x1);
}
void nodePool3D::getNodeTypeFlag(nodeTexel node, bool &flag)
{
	flag = (node.first>>30 & 0x1);
}
//set 
void nodePool3D::setNodePos(nodeTexel &node, int z, int y, int  x)
{
}

void nodePool3D::setBlockPos(nodeTexel &node, int z, int y, int x)
{
}
void nodePool3D::setRGBA(nodeTexel &node, int R, int G, int B, int A )
{
}
void nodePool3D::setMaxSubDivision(nodeTexel &node, int subD)
{
}
void nodePool3D::setNodeTypeFlag(nodeTexel &node, int flag)
{
}

//ALL in one
void nodePool3D::setBlockNodeTexel(nodeTexel &node, bool subD, unsigned int nz, unsigned int ny, unsigned int nx, unsigned int bz, unsigned int by, unsigned int bx)
{
	node.first = (subD<<31) | (1<<30) | (nz<<20) | (ny<<10) | nx;
	node.second = (bz<<20) | (by<<10) | bx;

}
//nz, ny, nx is nodeCluster position for this node's children
void nodePool3D::setSingleColorNodeTexel(nodeTexel &node, bool subD, int nz, int ny, int nx, int R, int G, int B, int A)
{
	node.first = (subD<<31) | (0<<30) | (nz<<20) | (ny<<10) | nx;
	node.second = (R<<24) | (G<<16) | (B<<8) | A;

}
void nodePool3D::setSingleColorNodeTexel(nodeTexel &node, bool subD, unsigned int nz, unsigned int ny, unsigned int nx, unsigned int singleValue)
{
	node.first = (subD<<31) | (0<<30) | (nz<<20) | (ny<<10) | nx;
	node.second = singleValue;

}

bool nodePool3D::writeToFile(const char* filename)
{
	FILE *pFile; 
	//fopen_s(&pFile, filename,"rb");
	std::string fname=std::string(filename);
	fname += std::string("-ddd-");
	fname += number2String(_dim*2);
	fname += std::string(".raw");

	pFile= fopen(fname.c_str(), "wb");
	if (NULL == pFile) {
		std::cout<<"can't open file!"<<std::endl;
		return false;
	}

	assert(_data);
	int size = fwrite(_data, sizeof(nodeTexel), _dim*2*_dim*2*_dim*2, pFile);
	if (size != _dim*2*_dim*2*_dim*2)
		return false;
	std::cout <<"write:"<<fname.c_str()<< std::endl;

	fclose(pFile);
	return true;
}

bool nodePool3D::readCompressedPoolFromFile(const char*filename, int dim)
{
	_dim = dim;
	FILE *pFile; 
	//fopen_s(&pFile, filename,"rb");
	pFile= fopen(filename, "rb");
	if (NULL == pFile) {
		std::cout<<"can't open file!"<<std::endl;
		return false;
	}

	_data = (nodeTexel*)malloc(_dim*2*_dim*2*_dim*2*sizeof(nodeTexel));
	int size = fread(_data, sizeof(nodeTexel), _dim*2*_dim*2*_dim*2, pFile);
	if (size != _dim*2*_dim*2*_dim*2)
		return false;
	std::cout <<"read:"<<filename<< std::endl;

	fclose(pFile);
	return true;
}


void nodePool3D::debugOutput()
{
	int count=0;
	for(int k=0; k<_dim; k++)
		for (int j=0; j<_dim; j++)
			for(int i=0; i<_dim; i++)
			{
				if(count==_count)
					break;
				count++;
				printf("SelfPointer---------: %d  %d %d \n",k,j,i);

				for(int kk=0; kk<2; kk++)
					for (int jj=0; jj<2; jj++)
						for(int ii=0; ii<2; ii++)
						{
							nodeMetaData metaData=getSingleMetaNode(k*2+kk, j*2+jj, i*2+ii);
							
							printf("ID:%3d ",metaData.ID);

							printf("ChildrenCluster: %d %d %d   ",
																			metaData.childrenNodeClusterPointer[2],
																			metaData.childrenNodeClusterPointer[1],
																			metaData.childrenNodeClusterPointer[0]);
							printf("Block Pointer: %d %d %d ", metaData.blockPointer[2],
																					metaData.blockPointer[1],
																					metaData.blockPointer[0]);
							printf("  subD: %d   type: %d \n",metaData.subD, metaData.type);

						}
				//printf("-------------- cluster end \n");
			}
	

}
void nodePool3D::debugOutputToFile(const char* filename)
{
	int count=0;
	for(int k=0; k<_dim; k++)
		for (int j=0; j<_dim; j++)
			for(int i=0; i<_dim; i++)
			{
				if(count==_count)
					break;
				count++;
				//printf("-------------- cluster begin \n");
				printf("SelfPointer---------: %d  %d %d \n",k,j,i);

				for(int ii=0; ii<2; ii++)
					for (int jj=0; jj<2; jj++)
						for(int kk=0; kk<2; kk++)
						{
							nodeMetaData metaData=getSingleMetaNode(k*2+kk, j*2+jj, i*2+ii);
							
							printf("ID: %3d",metaData.ID);

							printf(" ChildrenCluster: %d %d %d   ",
																			metaData.childrenNodeClusterPointer[2],
																			metaData.childrenNodeClusterPointer[1],
																			metaData.childrenNodeClusterPointer[0]);
							printf("Block Pointer: %d %d %d ", metaData.blockPointer[2],
																					metaData.blockPointer[1],
																					metaData.blockPointer[0]);
							printf("  subD: %d   type: %d \n",metaData.subD, metaData.type);

						}
				//printf("-------------- cluster end \n");
			}
	

}
